//
// Created by administered on 2025/7/15.
//
#include "backward.h"

// 计算两点间的距离
double distance_between_points(Point p1, Point p2) {
    double dx = p2.x - p1.x;
    double dy = p2.y - p1.y;
    return sqrt(dx * dx + dy * dy);
}

// 计算两点间的方位角(弧度)
double azimuth_between_points(Point p1, Point p2) {
    double dx = p2.x - p1.x;
    double dy = p2.y - p1.y;
    return atan2(dy, dx);
}

// 后方交会核心算法 - 角度交会法
CalculationResult resection_angle_method(Point* control_points,
                                         Observation* observations,
                                         int num_points,
                                         double initial_x, double initial_y) {
    CalculationResult result;
    result.success = 0;
    result.iterations = 0;

    // 初始猜测值
    Point current = {initial_x, initial_y, 0};

    // 迭代解算
    for (int iter = 0; iter < MAX_ITERATIONS; iter++) {
        result.iterations++;

        // 构建误差方程系数矩阵和常数项
        double A[2][2] = {0};    // 法方程系数矩阵
        double b[2] = {0};       // 法方程常数项

        // 遍历每个观测值
        for (int i = 0; i < num_points; i++) {
            for (int j = i + 1; j < num_points; j++) {
                // 计算当前点到控制点的距离和方位角
                double dist_i = distance_between_points(current, control_points[i]);
                double dist_j = distance_between_points(current, control_points[j]);
                double az_i = azimuth_between_points(current, control_points[i]);
                double az_j = azimuth_between_points(current, control_points[j]);

                // 计算观测角与计算角的差值
                double calc_angle = az_j - az_i;
                while (calc_angle > M_PI) calc_angle -= 2 * M_PI;
                while (calc_angle < -M_PI) calc_angle += 2 * M_PI;

                double angle_diff = calc_angle - observations[j].angle + observations[i].angle;

                // 计算偏导数
                double dx_i = control_points[i].x - current.x;
                double dy_i = control_points[i].y - current.y;
                double dx_j = control_points[j].x - current.x;
                double dy_j = control_points[j].y - current.y;

                double denominator_i = dx_i * dx_i + dy_i * dy_i;
                double denominator_j = dx_j * dx_j + dy_j * dy_j;

                double a_x = (dy_i / denominator_i) - (dy_j / denominator_j);
                double a_y = (dx_j / denominator_j) - (dx_i / denominator_i);

                // 构建法方程
                A[0][0] += a_x * a_x;
                A[0][1] += a_x * a_y;
                A[1][0] += a_x * a_y;
                A[1][1] += a_y * a_y;

                b[0] += a_x * angle_diff;
                b[1] += a_y * angle_diff;
            }
        }

        // 解法方程 A·ΔX = b
        double det = A[0][0] * A[1][1] - A[0][1] * A[1][0];
        if (fabs(det) < 1e-10) {
            // 矩阵接近奇异，可能是危险圆情况
            return result;
        }

        double delta_x = (A[1][1] * b[0] - A[0][1] * b[1]) / det;
        double delta_y = (A[0][0] * b[1] - A[1][0] * b[0]) / det;

        // 更新坐标
        current.x += delta_x;
        current.y += delta_y;

        // 检查收敛性
        if (fabs(delta_x) < CONVERGENCE_THRESHOLD &&
            fabs(delta_y) < CONVERGENCE_THRESHOLD) {
            result.success = 1;
            result.position = current;

            // 计算点位精度
            double sum_v2 = 0;
            int n = num_points * (num_points - 1) / 2;  // 观测方程数
            int t = 2;  // 未知数个数

            for (int i = 0; i < num_points; i++) {
                for (int j = i + 1; j < num_points; j++) {
                    double az_i = azimuth_between_points(current, control_points[i]);
                    double az_j = azimuth_between_points(current, control_points[j]);
                    double calc_angle = az_j - az_i;
                    while (calc_angle > M_PI) calc_angle -= 2 * M_PI;
                    while (calc_angle < -M_PI) calc_angle += 2 * M_PI;

                    double v = calc_angle - observations[j].angle + observations[i].angle;
                    sum_v2 += v * v;
                }
            }

            // 计算单位权中误差
            double sigma0 = sqrt(sum_v2 / (n - t));

            // 计算点位中误差
            double qxx = A[1][1] / det;
            double qyy = A[0][0] / det;
            result.error_x = sigma0 * sqrt(qxx);
            result.error_y = sigma0 * sqrt(qyy);
            result.error_rms = sigma0 * sqrt(qxx + qyy);

            break;
        }
    }

    return result;
}

// 危险圆检测
int is_dangerous_circle(Point* control_points, int num_points) {
    // 简化的危险圆检测，计算控制点是否接近共线
    if (num_points < 3) return 0;

    double max_area = 0;

    // 计算所有三点组合形成的三角形面积
    for (int i = 0; i < num_points; i++) {
        for (int j = i + 1; j < num_points; j++) {
            for (int k = j + 1; k < num_points; k++) {
                double area = fabs((control_points[i].x*(control_points[j].y-control_points[k].y) +
                                    control_points[j].x*(control_points[k].y-control_points[i].y) +
                                    control_points[k].x*(control_points[i].y-control_points[j].y)) / 2);

                if (area > max_area) max_area = area;
            }
        }
    }

    // 如果最大面积很小，说明控制点接近共线
    return (max_area < 1e-6);
}

// 角度交会法的包装函数，提供更友好的接口
CalculationResult calculate_resection(Point* control_points, Observation* observations, int num_points) {
    // 检查控制点数量
    if (num_points < 3) {
        CalculationResult result;
        result.success = 0;
        result.iterations = 0;
        return result;
    }

    // 检测危险圆
    if (is_dangerous_circle(control_points, num_points)) {
        CalculationResult result;
        result.success = 0;
        result.iterations = 0;
        return result;
    }

    // 计算控制点的重心作为初始猜测值
    double sum_x = 0, sum_y = 0;
    for (int i = 0; i < num_points; i++) {
        sum_x += control_points[i].x;
        sum_y += control_points[i].y;
    }

    double initial_x = sum_x / num_points;
    double initial_y = sum_y / num_points;

    // 调用角度交会法计算
    return resection_angle_method(control_points, observations, num_points, initial_x, initial_y);
}